#include "Localoptim.h"
#include "Optimizer.h"
#include "Converter.h"

#include<mutex>
#include<chrono>

namespace msf
{
/**
 * @brief 局部优化线程构造函数
 * @param pSys 系统类指针
 * @param pAtlas atlas
 * @param bInertial 是否是惯性模式
 */
LocalOptimizing::LocalOptimizing(System* pSys, Atlas *pAtlas, bool bInertial):
    mpSystem(pSys), mbInertial(bInertial), mbResetRequested(false), mbResetRequestedActiveMap(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas), bInitializing(false),
    mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyPoseFrames(true),
    mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), infoInertial(Eigen::MatrixXd::Zero(9,9))
{
      /*
      * mbStopRequested:    外部线程调用，为true，表示外部线程请求停止 local mapping
      * mbStopped:          为true表示可以并终止localmapping 线程
      * mbNotStop:          true，表示不要停止 localmapping 线程，因为要插入关键帧了。需要和 mbStopped 结合使用
      * mbAcceptKeyFrames:  true，允许接受关键帧。tracking 和local mapping 之间的关键帧调度
      * mbAbortBA:          是否流产BA优化的标志位
      * mbFinishRequested:  请求终止当前线程的标志。注意只是请求，不一定终止。终止要看 mbFinished
      * mbResetRequested:   请求当前线程复位的标志。true，表示一直请求复位，但复位还未完成；表示复位完成为false
      * mbFinished:         判断最终LocalMapping::Run() 是否完成的标志。
      */
      mbBadImu = false;

      mTinit = 0.f;

      mNumLM = 0;
      mNumKPFCulling=0;
} 

/**
 * @brief 设置跟踪类指针
 */
void LocalOptimizing::SetTracker(Tracking *pTracker)
{
    mpTracker=pTracker;
}

/**
 * @brief 局部地图线程主函数
 */
void LocalOptimizing::Run()
{
    // 标记状态，表示当前run函数正在运行，尚未结束
    mbFinished = false;

    // 主循环
    while(1)
    {
        // Tracking will see that Local Mapping is busy
        // Step 1 告诉Tracking，LocalOptimizing正处于繁忙状态，请不要给我发送关键帧打扰我
        // LocalOptimizing线程处理的关键帧都是Tracking线程发过来的
        SetAcceptKeyPoseFrames(false);        

        // Check if there are keyframes in the queue
        // 等待处理的关键帧列表不为空 并且imu正常
        if(CheckNewKeyPoseFrames() && !mbBadImu)
        {
            // Step 2 处理列表中的关键帧，包括插入到地图等
            ProcessNewKeyPoseFrame(); 

            // mbOnlyTracking模式，仅仅插入关键帧到地图为止，不进行后续优化处理
            if(!CheckNewKeyPoseFrames() && mpTracker->mbOnlyTracking)
            {
                continue;
            }

            // 注意orbslam2中放在了函数SearchInNeighbors（用到了mbAbortBA）后面，应该放这里更合适
            mbAbortBA = false;

            bool b_doneLBA = false;
            int num_FixedKF_BA = 0;
            int num_OptKF_BA = 0;
            int num_MPs_BA = 0;
            int num_edges_BA = 0;

            // 已经处理完队列中的最后的一个关键帧，并且闭环检测没有请求停止LocalOptimizing
            if(!CheckNewKeyPoseFrames() && !stopRequested())
            {
                // 当前地图中关键帧数目大于2个
                if(mpAtlas->KeyPoseFramesInMap()>2)
                {
                    // Step 6.1 处于IMU模式并且当前关键帧所在的地图已经完成IMU初始化
                    if(mbInertial && mpCurrentKeyPoseFrame->GetMap()->isImuInitialized())
                    {
                        // 计算上一关键帧到当前关键帧Odom光心的距离 + 上上关键帧到上一关键帧相机Odom的距离
                        float dist = (mpCurrentKeyPoseFrame->mPrevKPF->GetOdomCenter() - mpCurrentKeyPoseFrame->GetOdomCenter()).norm() +
                                (mpCurrentKeyPoseFrame->mPrevKPF->mPrevKPF->GetOdomCenter() - mpCurrentKeyPoseFrame->mPrevKPF->GetOdomCenter()).norm();
                        // 如果距离大于5厘米，记录当前KF和上一KF时间戳的差，累加到mTinit
                        if(dist>0.05)
                            mTinit += mpCurrentKeyPoseFrame->mTimeStamp - mpCurrentKeyPoseFrame->mPrevKPF->mTimeStamp;
                        // 当前关键帧所在的地图尚未完成IMU BA2（IMU第三阶段初始化）
                        if(!mpCurrentKeyPoseFrame->GetMap()->GetIniertialBA2())
                        {
                            // 如果累计时间差小于10s 并且 距离小于2厘米，认为运动幅度太小，不足以初始化IMU，将mbBadImu设置为true
                            // Problem13.1 距离小于2cm导致重启
                            //if((mTinit<10.f) && (dist<0.02))
                            if((mTinit<2.f))
                            {
                                cout << "LocalOptim THREAD: Not enough motion for initializing. Reseting..." << endl;
                                unique_lock<mutex> lock(mMutexReset);
                                mbResetRequestedActiveMap = true;
                                mpMapToReset = mpCurrentKeyPoseFrame->GetMap();
                                mbBadImu = true;  // 在跟踪线程里会重置当前活跃地图
                            }
                        }
                        // 局部地图+IMU一起优化，优化关键帧位姿、IMU参数
                        Optimizer::LocalInertialBA(mpCurrentKeyPoseFrame, &mbAbortBA, mpCurrentKeyPoseFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA, !mpCurrentKeyPoseFrame->GetMap()->GetIniertialBA2());
                        b_doneLBA = true;
                    }
                    else
                    {
                        // Step 6.2 不是IMU模式或者当前关键帧所在的地图还未完成IMU初始化
                        b_doneLBA = false;
                    }
                }

                // Initialize IMU here
                // Step 7 当前关键帧所在地图未完成IMU初始化（第一阶段）
                // IMU第一阶段初始化
                if(!mpCurrentKeyPoseFrame->GetMap()->isImuInitialized() && mbInertial)
                {
                    // 在函数InitializeIMU里设置IMU成功初始化标志 SetImuInitialized
                    // IMU第一次初始化
                    //InitializeIMU(1e2, 1e5, true); 
                    InitializeIMU(1.0e-12, 1.0e-9, true); //param a/g 1.0e-12, 1e-9 3.0
                }

                // Step 9 如果距离IMU第一阶段初始化成功累计时间差小于100s，进行OIBA
                // Problem14.1 轨迹优化漂移,暂时取消第二三次imu初始化
                if ((mTinit<20.0f) && mbInertial)// 25
                //if(0)
                {
                    // Step 9.1 根据条件判断是否进行OIBA1（IMU第二次初始化）
                    // IMU第二阶段初始化
                    // 条件：1、当前关键帧所在的地图完成IMU初始化---并且--------2、正常跟踪状态----------
                    if(mpCurrentKeyPoseFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-optimizing is called
                    {
                        // 当前关键帧所在的地图还未完成OIBA 1
                        if(!mpCurrentKeyPoseFrame->GetMap()->GetIniertialBA1())
                        {
                            // 如果累计时间差大于5s，开始OIBA1（IMU第二阶段初始化）
                            if (mTinit>5.0f)
                            {
                                cout << "LocalOptim THREAD: start OIBA 1" << endl;
                                mpCurrentKeyPoseFrame->GetMap()->SetIniertialBA1();
                                //InitializeIMU(1.f, 1e5, true);
                                InitializeIMU(1.0e-7, 1.0e-4, true); //param 1.0e-11,1.0e-8 1.0e-7, 1.0e-4
                                cout << "LocalOptim THREAD: end OIBA 1" << endl;
                            }
                        }

                        // Step 9.2 根据条件判断是否进行OIBA2（IMU第三次初始化）
                        // 当前关键帧所在的地图还未完成OIBA 2
                        /*
                        else if(!mpCurrentKeyPoseFrame->GetMap()->GetIniertialBA2())
                        {
                            if (mTinit>6.0f){
                                cout << "LocalOptim THREAD: start OIBA 2" << endl;
                                mpCurrentKeyPoseFrame->GetMap()->SetIniertialBA2();
                                //InitializeIMU(0.f, 0.f, true);
                                InitializeIMU(1.0e11, 1.0e14, true); //param 1.0, 1.0e3 / 1.0e11, 1.0e14
                                cout << "LocalOptim THREAD: end OIBA 2" << endl;
                            }
                        }
                        */

                        // scale refinement
                        // Step 9.3 ......
                    }
                }
            }
        }
        else if(Stop() && !mbBadImu) // 当要终止当前线程的时候
        {
            while(isStopped() && !CheckFinish())
            {
                // 如果还没有结束利索,那么等等它
                usleep(3000);
            }
            // 然后确定终止了就跳出这个线程的主循环
            if(CheckFinish())
                break;
        }
        // 查看是否有复位线程的请求
        ResetIfRequested();

        // Tracking will see that Local Optimizing is busy
        // 开始接收关键帧
        SetAcceptKeyPoseFrames(true);

        // 如果当前线程已经结束了就跳出主循环
        if(CheckFinish())
            break;

        usleep(3000);
    }

    // 设置线程已经终止
    SetFinish();    
}

/**
 * @brief 插入关键帧,由外部（Tracking）线程调用;这里只是插入到列表中,等待线程主函数对其进行处理
 * @param pKPF 新的关键帧
 */
void LocalOptimizing::InsertKeyPoseFrame(KeyPoseFrame *pKPF)
{
    unique_lock<mutex> lock(mMutexNewKPFs);
    // 将关键帧插入到列表中
    mlNewKeyPoseFrames.push_back(pKPF);
    mbAbortBA=true;
}

/**
 * @brief 查看列表中是否有等待被插入的关键帧
 */
bool LocalOptimizing::CheckNewKeyPoseFrames()
{
    unique_lock<mutex> lock(mMutexNewKPFs);
    return(!mlNewKeyPoseFrames.empty());
}

/**
 * @brief 处理列表中的关键帧，包括计算共视图，插入到地图等
 */
void LocalOptimizing::ProcessNewKeyPoseFrame()
{
    // Step 1：从缓冲队列中取出一帧关键帧
    // 该关键帧队列是Tracking线程向LocalOptimizing中插入的关键帧组成
    {
        unique_lock<mutex> lock(mMutexNewKPFs);
        // 取出列表中最前面的关键帧，作为当前要处理的关键帧
        mpCurrentKeyPoseFrame = mlNewKeyPoseFrames.front();
        // 取出最前面的关键帧后，在原来的列表里删掉该关键帧
        mlNewKeyPoseFrames.pop_front();
    }

    // Update links in the Covisibility Graph
    // Step 2：更新关键帧间的连接关系（共视图）
    // 获取最新的KPF，传入参数
    //vector<KeyPoseFrame*> v = mpAtlas->GetAllKeyPoseFrames();
    //KeyPoseFrame *lastKPF = v.empty() ? nullptr : v.back();
    mpCurrentKeyPoseFrame->UpdateConnections();

    // Insert Keyposeframe in Map
    // Step 3：将该关键帧插入到地图中
    mpAtlas->AddKeyPoseFrame(mpCurrentKeyPoseFrame);
}

/**
 * @brief 处理新的关键帧，使队列为空，注意这里只是处理了关键帧，并没有生成MP
 */
void LocalOptimizing::EmptyQueue()
{
    while(CheckNewKeyPoseFrames())
        ProcessNewKeyPoseFrame();
}

/**
 * @brief 外部线程调用,请求停止当前线程的工作; ORBSLAM中是回环检测线程调用,来避免在进行全局优化的过程中局部建图线程添加新的关键帧
 */
void LocalOptimizing::RequestStop()
{
    unique_lock<mutex> lock(mMutexStop);
    mbStopRequested = true;
    unique_lock<mutex> lock2(mMutexNewKPFs);
    mbAbortBA = true;
}

/**
 * @brief 检查是否要把当前的局部建图线程停止工作,运行的时候要检查是否有终止请求,如果有就执行. 由run函数调用
 */
bool LocalOptimizing::Stop()
{
    unique_lock<mutex> lock(mMutexStop);
    // 如果当前线程还没有准备停止,但是已经有终止请求了,那么就准备停止当前线程
    if(mbStopRequested && !mbNotStop)
    {
        mbStopped = true;
        cout << "Localoptim THREAD: Local Optimizing STOP" << endl;
        return true;
    }

    return false;
}

/**
 * @brief 检查mbStopped是否为true，为true表示可以并终止localoptimizing线程
 */
bool LocalOptimizing::isStopped()
{
    unique_lock<mutex> lock(mMutexStop);
    return mbStopped;
}

/**
 * @brief 求外部线程调用，为true，表示外部线程请求停止 local optimizing
 */
bool LocalOptimizing::stopRequested()
{
    unique_lock<mutex> lock(mMutexStop);
    return mbStopRequested;
}

/**
 * @brief 释放当前还在缓冲区中的关键帧指针
 */
void LocalOptimizing::Release()
{
    unique_lock<mutex> lock(mMutexStop);
    unique_lock<mutex> lock2(mMutexFinish);
    if(mbFinished)
        return;
    mbStopped = false;
    mbStopRequested = false;
    for(list<KeyPoseFrame*>::iterator lit = mlNewKeyPoseFrames.begin(), lend=mlNewKeyPoseFrames.end(); lit!=lend; lit++)
        delete *lit;
    mlNewKeyPoseFrames.clear();

    cout << "LocalOptim THREAD: Local Mapping RELEASE" << endl;
}

/**
 * @brief 查看是否接收关键帧，也就是当前线程是否在处理数据，当然tracking线程也不会全看这个值，他会根据队列阻塞情况
 */
bool LocalOptimizing::AcceptKeyPoseFrames()
{
    unique_lock<mutex> lock(mMutexAccept);
    return mbAcceptKeyPoseFrames;
}

/**
 * @brief 设置"允许接受关键帧"的状态标志
 */
void LocalOptimizing::SetAcceptKeyPoseFrames(bool flag)
{
    unique_lock<mutex> lock(mMutexAccept);
    mbAcceptKeyPoseFrames=flag;
}

/**
 * @brief 如果不让它暂停，即使发出了暂停信号也不暂停
 */
bool LocalOptimizing::SetNotStop(bool flag)
{
    unique_lock<mutex> lock(mMutexStop);

    if(flag && mbStopped)
        return false;

    mbNotStop = flag;

    return true;
}

/**
 * @brief 放弃这次操作，虽然叫BA但并不是只断优化
 */
void LocalOptimizing::InterruptBA()
{
    mbAbortBA = true;
}

/**
 * @brief 请求reset
 */
void LocalOptimizing::RequestReset()
{
    {
        unique_lock<mutex> lock(mMutexReset);
        cout << "LocalOptim THREAD: Optim reset recieved" << endl;
        mbResetRequested = true;
    }
    cout << "LocalOptim THREAD: Optim reset, waiting..." << endl;

    // 一直等到局部建图线程响应之后才可以退出
    while(1)
    {
        {
            unique_lock<mutex> lock2(mMutexReset);
            if(!mbResetRequested)
                break;
        }
        usleep(3000);
    }
    cout << "LocalOptim THREAD: Optim reset, Done!!!" << endl;
}

/**
 * @brief 接收重置当前地图的信号
 */
void LocalOptimizing::RequestResetActiveMap(Map* pMap)
{
      {
            unique_lock<mutex> lock(mMutexReset);
            cout << "LocalOptim THREAD: Active map reset recieved" << endl;
            mbResetRequestedActiveMap = true;
            mpMapToReset = pMap;
      }
      cout << "LocalOptim THREAD: Active map reset, waiting..." << endl;

      while(1)
      {
            {
                  unique_lock<mutex> lock2(mMutexReset);
                  if(!mbResetRequestedActiveMap)
                        break;
            }
            usleep(3000);
      }
      cout << "LocalOptim THREAD: Active map reset, Done!!!" << endl;
}

/**
 * @brief 检查是否有复位线程的请求
 */
void LocalOptimizing::ResetIfRequested()
{
    bool executed_reset = false;
    {
        unique_lock<mutex> lock(mMutexReset);
        // 执行复位操作:清空关键帧缓冲区
        if(mbResetRequested)
        {
            executed_reset = true;
            cout << "LocalOptim THREAD: Reseting Atlas in Local Optimizing..." << endl;
            mlNewKeyPoseFrames.clear();
            // 恢复为false表示复位过程完成
            mbResetRequested = false;
            mbResetRequestedActiveMap = false;

            // Inertial parameters
            mTinit = 0.f;
            mbNotBA2 = true;
            mbNotBA1 = true;
            mbBadImu=false;

            mIdxInit=0;

            cout << "LocalOptim THREAD: End reseting Local Optimizing..." << endl;
        }

        if(mbResetRequestedActiveMap) {
            executed_reset = true;
            cout << "LocalOptim THREAD: Reseting current map in Local Optimizing..." << endl;
            mlNewKeyPoseFrames.clear();

            // Inertial parameters
            mTinit = 0.f;
            mbNotBA2 = true;
            mbNotBA1 = true;
            mbBadImu=false;

            mbResetRequested = false;
            mbResetRequestedActiveMap = false;
            cout << "LocalOptim THREAD: End reseting Local Optimizing..." << endl;
        }
    }
    if(executed_reset)
        cout << "LocalOptim THREAD: Reset free the mutex" << endl;
}

/**
 * @brief 请求终止当前线程
 */
void LocalOptimizing::RequestFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinishRequested = true;
}

/**
 * @brief 查看完成信号，跳出while循环
 */
bool LocalOptimizing::CheckFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinishRequested;
}

/**
 * @brief 设置当前线程已经真正地结束了
 */
void LocalOptimizing::SetFinish()
{
    unique_lock<mutex> lock(mMutexFinish);
    mbFinished = true;    
    unique_lock<mutex> lock2(mMutexStop);
    mbStopped = true;
}

/**
 * @brief 当前线程的run函数是否已经终止
 */
bool LocalOptimizing::isFinished()
{
    unique_lock<mutex> lock(mMutexFinish);
    return mbFinished;
}

/** 
 * @brief imu初始化
 * @param priorG 陀螺仪偏置的信息矩阵系数，主动设置时一般bInit为true，也就是只优化最后一帧的偏置，这个数会作为计算信息矩阵时使用
 * @param priorA 加速度计偏置的信息矩阵系数
 * @param bFIBA 是否做BA优化，目前都为true
 */
void LocalOptimizing::InitializeIMU(float priorG, float priorA, bool bFIBA)
{
    // 1. 将所有关键帧放入列表及向量里，且查看是否满足初始化条件
    if (mbResetRequested)
        return;

    float minTime;
    int nMinKPF;

    // 从时间及帧数上限制初始化，不满足下面的不进行初始化
    // Problem14.6 IMU初始化不充分
    //minTime = 1.0;
    //nMinKPF = 10;

    minTime = 7.0;
    nMinKPF = 35;
    // 当前地图大于45帧才进行初始化
    if(mpAtlas->KeyPoseFramesInMap()<nMinKPF)
        return;
    
    // Retrieve all keyframe in temporal order
    // 按照顺序存放目前地图里的关键帧，顺序按照前后顺序来，包括当前关键帧
    list<KeyPoseFrame*> lpKPF; // old -> current
    KeyPoseFrame* pKPF = mpCurrentKeyPoseFrame;
    while(pKPF->mPrevKPF)
    {
        lpKPF.push_front(pKPF);
        pKPF = pKPF->mPrevKPF;
    }
    lpKPF.push_front(pKPF);
    // 同样内容再构建一个和lpKPF一样的容器vpKPF
    vector<KeyPoseFrame*> vpKPF(lpKPF.begin(),lpKPF.end());
    if(vpKPF.size()<nMinKPF)
        return;

    mFirstTs=vpKPF.front()->mTimeStamp;
    if(mpCurrentKeyPoseFrame->mTimeStamp-mFirstTs<minTime)
        return;

    // 正在做IMU的初始化，在tracking里面使用，如果为true，暂不添加关键帧
    bInitializing = true;

    // 先处理新关键帧，防止堆积且保证数据量充足
    while(CheckNewKeyPoseFrames())
    {
        ProcessNewKeyPoseFrame();
        vpKPF.push_back(mpCurrentKeyPoseFrame);
        lpKPF.push_back(mpCurrentKeyPoseFrame);
    }

    // 2. 正式IMU初始化
    const int N = vpKPF.size();
    IMU::Bias b(0,0,0,0,0,0); //ba xyz bw xyz

    // 在IMU连一次初始化都没有做的情况下
    if (!mpCurrentKeyPoseFrame->GetMap()->isImuInitialized())
    {
        Eigen::Matrix3f Rwg;
        Eigen::Vector3f dirG;
        dirG.setZero();

        int have_imu_num = 0;
        for(vector<KeyPoseFrame*>::iterator itKPF = vpKPF.begin(); itKPF!=vpKPF.end(); itKPF++)
        {
            if (!(*itKPF)->mpImuPreintegrated)
                continue;
            if (!(*itKPF)->mPrevKPF)
                continue;

            have_imu_num++;
            // 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij)
            dirG -= (*itKPF)->mPrevKPF->GetImuRotation() * (*itKPF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
            // 求取实际的速度，位移/时间
            Eigen::Vector3f _vel = ((*itKPF)->GetImuPosition() - (*itKPF)->mPrevKPF->GetImuPosition())/(*itKPF)->mpImuPreintegrated->dT;
            (*itKPF)->SetVelocity(_vel);
            (*itKPF)->mPrevKPF->SetVelocity(_vel);
        }

        // Problem14.6 IMU初始化不充分
        if (have_imu_num < 30)
        {
            cout << "LocalOptim THREAD: imu初始化失败, 由于带有imu预积分信息的关键帧数量太少" << endl;
            bInitializing=false;
            //mbBadImu = true; // 取消重置地图
            return;
        }

        // dirG = sV1 - sVn + n*Rwg*g*t
        // 归一化，约等于重力在世界坐标系下的方向
        dirG = dirG/dirG.norm();
        // 原本的重力方向
        Eigen::Vector3f gI(0.0f, 0.0f, -1.0f);
        // 求重力在世界坐标系下的方向与重力在重力坐标系下的方向的叉乘
        Eigen::Vector3f v = gI.cross(dirG);
        // 求叉乘模长
        const float nv = v.norm();
        // 求转角大小
        const float cosg = gI.dot(dirG);
        const float ang = acos(cosg);
        // v/nv 表示垂直于两个向量的轴  ang 表示转的角度，组成角轴
        Eigen::Vector3f vzg = v*ang/nv;
        // 获得重力坐标系到世界坐标系的旋转矩阵的初值
        Rwg = Sophus::SO3f::exp(vzg).matrix();
        mRwg = Rwg.cast<double>();
        mTinit = mpCurrentKeyPoseFrame->mTimeStamp-mFirstTs;
    }
    else
    {
        mRwg = Eigen::Matrix3d::Identity();
        mbg = mpCurrentKeyPoseFrame->GetGyroBias().cast<double>();
        mba = mpCurrentKeyPoseFrame->GetAccBias().cast<double>();        
    }

    mScale=1.0;

    // 暂时没发现在别的地方出现过
    mInitTime = mpTracker->mLastPoseFrame.mTimeStamp-vpKPF.front()->mTimeStamp;

    std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
    // 3. 计算残差及偏置差，优化尺度重力方向及速度偏置，偏置先验为0，Odom时不优化尺度
    Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, infoInertial, false, false, priorG, priorA);
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

    // 计算并转换时间差
    //auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
    // 打印时间差
    //std::cout << "LocalOptim THREAD : The time difference is " << duration << " milliseconds." << std::endl;

    // 尺度太小的话初始化认为失败
    if (mScale<1e-1)
    {
        cout << "scale too small" << endl;
        bInitializing=false;
        return;
    }

    // 到此时为止，前面做的东西没有改变map
    {
        unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
        // 尺度变化超过设定值，或者Odom+IMU时
        if ((fabs(mScale - 1.f) > 0.00001) || mbInertial) {
            // 4.1 恢复重力方向与尺度信息
            Sophus::SE3f Twg(mRwg.cast<float>().transpose(), Eigen::Vector3f::Zero()); // Tgw
            mpAtlas->GetCurrentMap()->ApplyScaledRotation(Twg, mScale, true);
            // 4.2 更新普通帧的位姿，主要是当前帧与上一帧
            mpTracker->UpdatePoseFrameIMU(mScale, vpKPF[0]->GetImuBias(), mpCurrentKeyPoseFrame);
        }

        // Check if initialization OK
        // 即使初始化成功后面还会执行这个函数重新初始化
        // 在之前没有初始化成功情况下（此时刚刚初始化成功）对每一帧都标记，后面的kf全部都在tracking里面标记为true
        // 也就是初始化之前的那些关键帧即使有imu信息也不算
        if (!mpAtlas->isImuInitialized())
            for (int i = 0; i < N; i++) {
                KeyPoseFrame *pKPF2 = vpKPF[i];
                pKPF2->bImu = true;
            }
    }

    // FullInertialBA不直接更新位姿了
    //mpTracker->UpdatePoseFrameIMU(1.0,vpKPF[0]->GetImuBias(),mpCurrentKeyPoseFrame);

    // 设置经过初始化了
    if (!mpAtlas->isImuInitialized())
    {
        // !重要！标记初始化成功
        mpAtlas->SetImuInitialized();
        mpTracker->t0IMU = mpTracker->mCurrentPoseFrame.mTimeStamp;
        mpCurrentKeyPoseFrame->bImu = true;
    }

    std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();

    // 代码里都为true
    if(bFIBA)
    {
        // 5. 承接上一步纯imu优化，按照之前的结果更新了尺度信息及适应重力方向，所以要结合地图进行一次odom加imu的全局优化
        // ! 1.0版本里面不直接赋值，而是将所有优化后的信息保存到变量里面
        if (priorA!=0.f)
            Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 50, false, mpCurrentKeyPoseFrame->mnId, NULL, true, priorG, priorA);
        else
            Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 50, false, mpCurrentKeyPoseFrame->mnId, NULL, false);
    }

    std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now();

    std::cout << "LocalOptim THREAD: IMUInitialize: Global Bundle Adjustment finished\nUpdating map ..." << endl;

    // Get Map Mutex
    unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);

    unsigned long GBAid = mpCurrentKeyPoseFrame->mnId;

    // Process keyframes in the queue
    // 6. 处理一下新来的关键帧，这些关键帧没有参与优化，但是这部分bInitializing为true，只在第2次跟第3次初始化会有新的关键帧进来
    // 这部分关键帧也需要被更新
    while(CheckNewKeyPoseFrames())
    {
        ProcessNewKeyPoseFrame();
        vpKPF.push_back(mpCurrentKeyPoseFrame);
        lpKPF.push_back(mpCurrentKeyPoseFrame);
    }
   
    // 7. 更新位姿
    // 获取地图中初始关键帧，第一帧肯定经过修正的
    // Problem15 不需要回环，所以这里不进行位姿更新了
    /*
    list<KeyPoseFrame*> lpKPFtoCheck(mpAtlas->GetCurrentMap()->mvpKeyPoseFrameOrigins.begin(),mpAtlas->GetCurrentMap()->mvpKeyPoseFrameOrigins.end());

    // 初始就一个关键帧，顺藤摸瓜找到父子相连的所有关键帧
    // 类似于树的广度优先搜索，其实也就是根据父子关系遍历所有的关键帧，有的参与了FullInertialBA有的没参与

    while(!lpKPFtoCheck.empty())
    {
        // 7.1 获得这个关键帧的子关键帧
        KeyPoseFrame* pKPF = lpKPFtoCheck.front();
        const set<KeyPoseFrame*> sChilds = pKPF->GetChilds();
        Sophus::SE3f Two = pKPF->GetPoseInverse();  // 获得关键帧的优化前的位姿

        // 7.2 遍历这个关键帧所有的子关键帧
        for(set<KeyPoseFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
        {
            // 确认是否能用
            KeyPoseFrame* pChild = *sit;
            if(!pChild || pChild->isBad())
                continue; 

            // 这个判定为true表示pChild没有参与前面的优化，因此要根据已经优化过的更新，结果全部暂存至变量
            if(pChild->mnBAGlobalForKPF!=GBAid)
            {
                // pChild->GetPose()也是优化前的位姿，Two也是优化前的位姿
                // 7.3 因此他们的相对位姿是比较准的，可以用于更新pChild的位姿
                Sophus::SE3f Tchildc = pChild->GetPose() * Two;
                // 使用相对位姿，根据pKPF优化后的位姿更新pChild位姿，最后结果都暂时放于mTowGBA
                pChild->mTowGBA = Tchildc * pKPF->mTowGBA;

                // 7.4 使用相同手段更新速度
                Sophus::SO3f Rcor = pChild->mTowGBA.so3().inverse() * pChild->GetPose().so3();
                if(pChild->isVelocitySet())
                {
                    pChild->mVwbGBA = Rcor * pChild->GetVelocity();
                }
                else 
                {
                    std::cout << "LocalOptim THREAD : IMUInitialize : Child velocity empty!!" << std::endl;
                }

                pChild->mBiasGBA = pChild->GetImuBias();
                pChild->mnBAGlobalForKPF = GBAid;
            }
            // 加入到list中，再去寻找pChild的子关键帧
            lpKPFtoCheck.push_back(pChild);
        }

        // 7.5 此时pKPF的利用价值就没了，但是里面的数值还都是优化前的，优化后的全部放于类似mTowGBA这样的变量中
        // 所以要更新到正式的状态里，另外mTowBefGBA要记录更新前的位姿，用于同样的手段更新三维点用
        pKPF->mTowBefGBA = pKPF->GetPose();
        pKPF->SetPose(pKPF->mTowGBA);

        // 速度偏置同样更新
        if(pKPF->bImu)
        {
            pKPF->mVwbBefGBA = pKPF->GetVelocity();
            pKPF->SetVelocity(pKPF->mVwbGBA);
            pKPF->SetNewBias(pKPF->mBiasGBA);
        } else {
            cout << "LocalOptim THREAD : IMUInitialize : KPF " << pKPF->mnId << " not set to inertial!! \n";
        }

        // pop
        lpKPFtoCheck.pop_front();        
    }
    */

    std::cout << "LocalOptim THREAD : IMUInitialize : Map updated!" << std::endl;

    mnKPFs=vpKPF.size();
    mIdxInit++;

    // 9. 再有新的来就不要了~不然陷入无限套娃了
    for(list<KeyPoseFrame*>::iterator lit = mlNewKeyPoseFrames.begin(), lend=mlNewKeyPoseFrames.end(); lit!=lend; lit++)
    {
        (*lit)->SetBadFlag();
        delete *lit;
    }
    mlNewKeyPoseFrames.clear();

    mpTracker->mState=Tracking::OK;
    bInitializing = false;

    mpCurrentKeyPoseFrame->GetMap()->IncreaseChangeIndex();

    return;
}

/**
 * @brief 通过BA优化进行尺度更新，关键帧小于100，使用了所有关键帧的信息，但只优化尺度和重力方向。每10s在这里的时间段内时多次进行尺度更新
 */
void LocalOptimizing::ScaleRefinement()
{




    return;
}

/**
 * @brief 返回是否正在做IMU的初始化，在tracking里面使用，如果为true，暂不添加关键帧
 */
bool LocalOptimizing::IsInitializing()
{
    return bInitializing;
}

/**
 * @brief 获取当前关键帧的时间戳，System::GetTimeFromIMUInit()中调用
 */
double LocalOptimizing::GetCurrKPFTime()
{

    if (mpCurrentKeyPoseFrame)
    {
        return mpCurrentKeyPoseFrame->mTimeStamp;
    }
    else
        return 0.0;
}

/**
 * @brief 获取当前关键帧
 */
KeyPoseFrame* LocalOptimizing::GetCurrKPF()
{
    return mpCurrentKeyPoseFrame;
}

}